#include "Armor.h"

void ArmorDetector::findLights()
{
    vector<vector<Point>> lightContours;
    Mat contourImg;
    srcImg_binary.copyTo(contourImg);
    findContours(contourImg, lightContours, 0, 2); //最耗时的操作，优化方向
    RotatedRect lightRect;
    LightBar light;
    for (const auto& lightContour : lightContours)
    {
        if (lightContour.size() < 6) continue; //轮廓点数小于6
        if (contourArea(lightContour) < armorParam.min_area) continue;

        lightRect = fitEllipse(lightContour);
        light = LightBar(lightRect);

        if (abs(light.angle) > armorParam.max_angle) continue;

        lights.emplace_back(light);
    }
    if (lights.size() < 2)
    {
        states = LIGHTS_NOT_FOUND;
        return;
    }

    sort(lights.begin(), lights.end(),
         [](LightBar& a1, LightBar& a2)
         {return a1.center.x < a2.center.x;});
    states = LIGHTS_FOUND;
    return;

}
